#include <assert.h>
#include <vector>

#include <ros/ros.h>

#include <Eigen3/Dense>

#include "mzslam.h"

using namespace std;
using namespace Eigen3;

namespace mzslam
{

void MzSLAM::_convert_IRs(const navigation::IR_conv::ConstPtr &msg, Eigen3::MatrixXf &ir)
{
  // distance limitation
  const float L_MAX = 50;
  const float S_MAX = 30;
  const float F_OFFSET = 10;
  const float LR_OFFSET = 9;
  ir(FRONT, L) = (msg->IR5 < L_MAX)? msg->IR5+F_OFFSET: -1;
  ir(FRONT, R) = (msg->IR2 < S_MAX)? msg->IR2+F_OFFSET: -1;
  ir(FRONT, 2) = 0.5 * (ir(FRONT, L) + ir(FRONT, R)); // not used
  ir(LEFT, F) = (msg->IR1 < S_MAX)? msg->IR1+LR_OFFSET: -1;
  ir(LEFT, M) = (msg->IR3 < L_MAX)? msg->IR3+LR_OFFSET: -1;
  ir(LEFT, B) = (msg->IR7 < S_MAX)? msg->IR7+LR_OFFSET: -1;
  ir(RIGHT, F) = (msg->IR4 < S_MAX)? msg->IR4+LR_OFFSET: -1;
  ir(RIGHT, M) = (msg->IR6 < L_MAX)? msg->IR6+LR_OFFSET: -1;
  ir(RIGHT, B) = (msg->IR0 < S_MAX)? msg->IR0+LR_OFFSET: -1;


  // some other limitations?
}

// add scan points from wall info
void MzSLAM::_convert_wall_to_scan(Eigen3::Matrix2f &wall, sensor_msgs::LaserScan &scan)
{
  const int num_readings = 56;
  const int angle_min = -50;
  const int angle_max = 230;

  scan.header.stamp = ros::Time::now(); // @todo should be synchronized with IRs?
  scan.header.frame_id = "/robot/ir_scan";
  scan.angle_min = _deg2rad(angle_min);
  scan.angle_max = _deg2rad(angle_max);
  scan.angle_increment = (scan.angle_max-scan.angle_min) / (float)num_readings;
  //scan.scan_time = ;
  //scan.time_increment = ;
  scan.range_min = 5.0;
  scan.range_max = 60.0;

  if(static_cast<int>(scan.ranges.size()) < num_readings) 
    scan.ranges.resize(num_readings);
  FOR(i, 0, num_readings) {
    float angle = scan.angle_min + i * scan.angle_increment;
    float ret = _calc_scan_ranges(angle, wall);
    if(ret>0) scan.ranges[i] = ret;
  }
}

float MzSLAM::_calc_scan_ranges(float angle, Eigen3::Matrix2f wall)
{
  float a1 = atan2(wall(0, 1), wall(0, 0));
  float a2 = atan2(wall(1, 1), wall(1, 0));
  a1 += (a1 < -3.14/2.0)? 3.14*2.0: 0;
  a2 += (a2 < -3.14/2.0)? 3.14*2.0: 0;
  //ROS_INFO("%f, %f", _rad2deg(a1), _rad2deg(a2));
  if(a1 > a2) {
    ROS_WARN("wall conversion: angle error??");
    return -1.0;
  }

  if(angle < a1 || a2 < angle) return -1.0;
  else
  {
    // calculate distance to intersection
    // AX=U
    Vector2f X;
    Matrix2f A;
    Vector2f U;
    A << tan(angle), -1, wall(1, 1)-wall(0, 1), -(wall(1, 0)-wall(0, 0));
    U << 0, -A.row(1)*wall.row(0).transpose();
    X = A.inverse() * U;
    return X.norm();
  }

  // won't come here
  return 0.0;
}

bool MzSLAM::estimate_wall(Eigen3::MatrixXf &_ir, int INDEX, Eigen3::Matrix2f &wall, Eigen3::MatrixXf &z)
{
  VectorXf ir = _ir.row(INDEX);
  if(ir.minCoeff() < 0) {
    ROS_DEBUG("wall estimation error: ir reading incorrect");
    return false;
  }

  // calculate optimized line
  VectorXf y(3);
  float dist;
  switch(INDEX) {
    case FRONT: y << -10, 10, 0; break;
    default: y << 10, 0, -10; break;
  }
  Vector2f coeff; /* a, b */
  coeff << 1, 0;
  float err = 0;
  _calc_optimized_line(ir, y, coeff, dist, err);

  // validate if the line fits the points
  const float ep = 5.0; // tekito
  if(err < ep) 
  {
    if(coeff(0) == 0) wall << coeff(1), 10, coeff(1), -10;
    else wall << (10-coeff(1))/coeff(0), 10, (-10-coeff(1))/coeff(0), -10;
  } 
  else 
  {
    ROS_DEBUG("wall estimation error: fitting error too large");
    return false;
  }

  // transform coordinate
  _transform_coordinate(wall, INDEX);

  // update measurement matrix
  MatrixXf _z(1, 2);
  _z << atan2(wall(1, 1)-wall(0, 1), wall(1, 0)-wall(0, 0)), dist;
  z.conservativeResize(z.rows()+1, z.cols());
  z.row(z.rows()-1) = _z;

  return true;
}

void MzSLAM::_calc_optimized_line(Eigen3::VectorXf &x, Eigen3::VectorXf &y,
    Eigen3::Vector2f &coeff, float &dist, float &err)
{
  Vector3f I; I << 1, 1, 1;
  float mean = (x.sum() / x.size());
  float sx2 = (x-mean*I).squaredNorm();
  float sxy = ( (x-mean*I).transpose()*y ).sum();

  if(sx2 == 0) 
  {
    // fitted line: x = mean
    coeff(0) = 0;
    coeff(1) = mean;
    dist = mean;

    // calculate error
    err += (x-mean*I).squaredNorm();
  } 
  else 
  {
    // fitted line: y = a*x + b
    float a = sxy / sx2;
    float b = - a * mean;
    coeff << a, b;

    dist = fabs(b) / sqrt(pow(a, 2)+1);

    // calculate error
    // @todo change it in a strict way?
    err += (x-(y-b*I)/a).squaredNorm();
  }
}

void MzSLAM::_transform_coordinate(Eigen3::Matrix2f &m, int i) {
  Matrix2f A;
  switch(i) {
    case FRONT:
      A << 0, 1, 1, 0;
      m = m * A;
      break;
    case LEFT:
      A << -1, 0, 0, 1;
      m = m * A;
      break;
    case RIGHT:
      Vector2f tmp = m.row(0);
      m.row(0) = m.row(1);
      m.row(1) = tmp;
      break;
  }
  A << cos(_deg2rad(-90)), -sin(_deg2rad(-90)), sin(_deg2rad(-90)), cos(_deg2rad(-90));
  m = A * m;
}

} // end of namespace
